#include "PhyEngine.h"


PhyEngine::PhyEngine(void)
{
}

PhyEngine::PhyEngine( RigidBodyList* bodies, int num_collision_steps, bool edge_intersection )
{
	this->bodies = bodies;
	this->edge_intersection = edge_intersection;
}


PhyEngine::~PhyEngine(void)
{
}

void PhyEngine::update( double dt )
{
	if (dt < 1e-8 || bodies == 0)
	{
		return;
	}

	for (int i=0; i<bodies->size(); i++)
	{
		bodies->at(i)->resetForce();
	}

	//processAllCollisions(dt);
	processCollisions(dt);

	generateGravity();

	//bodies->at(0)->applyForce(Vec3d(3, -5, 3), Vec3d(1, 0, 0));
	//bodies->at(0)->applyForce(Vec3d(-3, -11, -3), Vec3d(-1, 0, 0));

	for (int i=0; i<bodies->size(); i++)
	{
		bodies->at(i)->updateVel(dt);
	}

	//processAllCollisions(dt);

	for (int i=0; i<bodies->size(); i++)
	{
		bodies->at(i)->updatePos(dt);
	}
}

void PhyEngine::processCollisions( double dt )
{
	for (int i=0; i<bodies->size(); i++)
	{
		bodies->at(i)->rebuildMatrices();
	}

	for (int i=0; i<bodies->size(); i++)
	{
		for (int j=i+1; j<bodies->size(); j++)
		{
			RigidBody* b0 = bodies->at(i);
			RigidBody* b1 = bodies->at(j);

			CollisionPointList* cps0 = checkCollision(b0, b1);
			CollisionPointList* cps1 = checkCollision(b1, b0);

			if (cps0 != 0)
			{
				//printf("%d @\n", cps0->size());

				resolveCollision(b0, b1, cps0);

				cps0->clear();

				delete cps0;
			}

			if (cps1 != 0)
			{
				//printf("%d @\n", cps1->size());

				resolveCollision(b1, b0, cps1);

				cps1->clear();

				delete cps1;
			}

			
		}
	}
}

CollisionPointList* PhyEngine::checkCollision( RigidBody* b0, RigidBody* b1 )
{
	if (b0->mass > 1e8 && b1->mass > 1e8)
	{
		return 0;
	}

	if (mag2(b0->pos - b1->pos) > (b0->radius + b1->radius) * (b0->radius + b1->radius))
	{
		return 0;
	}

	CollisionPointList* cps = new CollisionPointList();	

	for (int j=0; j<b0->poly->vertCount; j++)
	{
		Vert v = b0->poly->vertList[j];

		Vec3d p = b1->getPointWorldToBody(b0->getPointBodyToWorld(Vec3d(v.x, v.y, v.z)));

		if (b1->evaluateSDM(p, false).d <= 0.0)
		{
			cps->push_back(Vec3d(v.x, v.y, v.z));
		}
	}

	//printf("%d\n", cp->at(i)->size());

	if (edge_intersection)
	{

		double resolution = 3;

		for (int j=0; j<b0->poly->lineCount; j++)
		{
			Line l = b0->poly->lineList[j];
			Vert v0 = b0->poly->vertList[l.a];
			Vert v1 = b0->poly->vertList[l.b];

			Vec3d edge_vector = b1->getVectorWorldToBody(b0->getVectorBodyToWorld(Vec3d(v1.x, v1.y, v1.z) - Vec3d(v0.x, v0.y, v0.z)));
			double edge_length = mag(edge_vector);
			int n = (int)(edge_length / resolution) - 1;
			//resolution = edge_length / n;
			Vec3d inc = edge_vector * (resolution /edge_length);
			Vec3d p = b1->getPointWorldToBody(b0->getPointBodyToWorld(Vec3d(v0.x, v0.y, v0.z)));
							

			for (; n>0; n--)
			{
				p += inc;
				Value curCP = b1->evaluateSDM(p, false);

				if (curCP.d <=0 && abs(curCP.d - 1e8) > 1e-8)
				{
					cps->push_back(b0->getPointWorldToBody(b1->getPointBodyToWorld(p)));
				}
			}
		}

	}

	//printf("%d\n", cp->at(i)->size());

	if (cps->size() == 0)
	{
		delete cps;

		return 0;
	}

	return cps;
}

void PhyEngine::resolveCollision( RigidBody* b0, RigidBody* b1, CollisionPointList* cps )
{
	for (int i=0; i<cps->size(); i++)
	{
		

		Value cp = b1->evaluateSDM(b1->getPointWorldToBody(b0->getPointBodyToWorld(cps->at(i))), true);			

		cp.p = b1->getPointBodyToWorld(cp.p);
		cp.n = b1->getVectorBodyToWorld(cp.n);

		//Vec3d point = cp.p;

		//printf("%f %f %f ### ", point[0], point[1], point[2]);

		//point = cp.n;

		//printf("%f %f %f\n", point[0], point[1], point[2]);

		generateForce(cp.p, cp.n, cp.d, 1, b0, b1);
	}
}

void PhyEngine::generateForce( Vec3d p, Vec3d n, double d, double r, RigidBody* b0, RigidBody* b1 )
{
	Vec3d vRelative = b0->getWorldVelocity(p) - b1->getWorldVelocity(p);
	double vRelativeN = dot(vRelative, n);
	Vec3d vRelativeT = vRelative - vRelativeN * n;

	Vec3d force = abs(d) * 100 * n - vRelativeN * 10 * n - vRelativeT * 10;

	b0->applyForce(p, force);
	b1->applyForce(p, -1.0 * force);
}

void PhyEngine::generateGravity()
{
	for (int i=0; i<bodies->size(); i++)
	{
		RigidBody* body = bodies->at(i);

		body->applyForce(body->pos, Vec3d(0, -3 * body->mass, 0));
	}
}
